#include "pid.h"

#include "board.h"
//#include <stdio.h>

PID::PID(float* _input, float* _feedback, float _kp, float _ki, float _kd, unsigned short _max_output)
{
		p_input=_input; 
		p_feedback=_feedback;
		m_kp=_kp;
		m_ki=_ki; 
		m_kd=_kd; 
		m_max_output=_max_output*1.0;
    clear();
}


void PID::clear(void){
    m_error = 0.0f;
		m_integra = 0.0f;
		m_derivative = 0.0f;
		m_previous_error =0.0f;
}

void PID::update(float _kp, float _ki, float _kd, unsigned short _max_output){
	m_kp = _kp;
	m_ki = _ki;
	m_kd = _kd;
	m_max_output = _max_output;
}

float PID::compute(float interval){
    //error = *input - *feedback;
	float val=0.0f;
	m_error = *p_input;

	m_integra = m_integra + m_error*interval;
	m_derivative = (m_error - m_previous_error) / interval;
	m_previous_error = m_error;
	
	if (m_ki != 0)
	#if PID_DEBUG_OUTPUT
		printf("integra=%0.3f max_output=%0.3f %0.3f ", integra, -max_output/ki, max_output/ki);
	#endif
	if (m_integra < -(m_max_output/m_ki))
	{
		//printf("integra clear-\r\n");
		m_integra = -(m_max_output/m_ki);
	}
	if (m_integra > m_max_output/m_ki)
	{
		//printf("integra clear+\r\n");
		m_integra = m_max_output/m_ki;
	}
	//float val = -error*kp - integra*ki - derivative*kd;
	val = -m_error*m_kp;

	if (val < -m_max_output)
		val = -m_max_output+1;
	else if (val > m_max_output)
		val = m_max_output-1;

#if PID_DEBUG_OUTPUT
	printf("error=%0.3f integra=%0.3f derivative=%0.3f val=%d\r\n", error, integra, derivative, val);
#endif

	return val;
}
